#include "RobotDelta.h"
#include  <cmath>

RobotDelta::RobotDelta()
{
}

RobotDelta::~RobotDelta()
{
}

void RobotDelta::SetDeltaPara(double pBETA1, double pBETA2, double pBETA3, double pRa, double pRb, 
	                          double pL1, double pL2, double pL3, double pZeroAngle)
{
	m_BETA1 = pBETA1;
	m_BETA2 = pBETA2;
	m_BETA3 = pBETA3;
	m_ra = pRa;
	m_rb = pRb;
	m_l1 = pL1;
	m_l2 = pL2;
	m_l3 = pL3;
	m_zero_angle = pZeroAngle;
}

void RobotDelta::TransPosToTheta(double pX, double pY, double pZ, double& rTh1, double& rTh2, double& rTh3)
{
	Delta_Inverse(m_BETA3, m_rb, pZ, m_l2, m_ra, m_BETA2, m_l3, pY, m_l1, m_BETA1, pX, m_zero_angle, &rTh3, &rTh1, &rTh2);
}

void RobotDelta::TransThetaToPos(double pTh1, double pTh2, double pTh3, double& rX, double& rY, double& rZ)
{
	Delta_Forward(m_l1, m_l3, pTh3, pTh2, m_rb, m_l2, m_ra, m_BETA3, m_BETA2, m_BETA1, pTh1, m_zero_angle, &rZ, &rY, &rX);
}

void RobotDelta::Delta_Inverse(double BETA3, double rb, double z, double l2, double ra,
	                           double BETA2, double l3, double y, double l1, double BETA1,
	                           double x, double zero_angle, double* th3, double* th1, double* th2)
{
    double a;
    double a_tmp;
    double b_a;
    double b_a_tmp;
    double beta1;
    double beta2;
    double beta3;
    double c_a;
    double c_a_tmp;
    double d_a;
    double d_a_tmp;
    double e_a;
    double e_a_tmp;
    double f_a_tmp;
    double g_a_tmp;
    double h_a_tmp;
    double i_a_tmp;
    double j_a_tmp;
    double k_a_tmp;
    double l_a_tmp;
    double m_a_tmp;
    double n_a_tmp;
    double o_a_tmp;
    double p_a_tmp;
    double q_a_tmp;
    double theta1_tmp_tmp;

    beta1 = BETA1 * 3.1415926535897931 / 180.0;
    beta2 = BETA2 * 3.1415926535897931 / 180.0;
    beta3 = BETA3 * 3.1415926535897931 / 180.0;
    theta1_tmp_tmp = std::cos(beta1);
    beta1 = std::sin(beta1);
    a_tmp = (x + ra * theta1_tmp_tmp) - rb * theta1_tmp_tmp;
    b_a_tmp = (y + ra * beta1) - rb * beta1;
    c_a_tmp = a_tmp - 1.7320508075688772 * l3 * theta1_tmp_tmp / 2.0;
    d_a_tmp = b_a_tmp - 1.7320508075688772 * l3 * beta1 / 2.0;
    e_a_tmp = 2.0 * l1 * theta1_tmp_tmp * c_a_tmp;
    f_a_tmp = 2.0 * l1 * beta1 * d_a_tmp;
    a = e_a_tmp + f_a_tmp;
    g_a_tmp = l3 / 2.0 + z;
    b_a = a_tmp - 0.8660254037844386 * l3 * theta1_tmp_tmp;
    c_a = b_a_tmp - 0.8660254037844386 * l3 * beta1;
    a_tmp = 0.5 * l3 + z;
    b_a_tmp = l1 * l1;
    h_a_tmp = l2 * l2;
    a_tmp *= a_tmp;
    b_a = (((b_a * b_a + c_a * c_a) + b_a_tmp) - h_a_tmp) + a_tmp;
    beta1 = std::cos(beta2);
    theta1_tmp_tmp = std::sin(beta2);
    beta2 = (x + ra * beta1) - rb * beta1;
    i_a_tmp = (y + ra * theta1_tmp_tmp) - rb * theta1_tmp_tmp;
    j_a_tmp = beta2 - 1.7320508075688772 * l3 * beta1 / 2.0;
    k_a_tmp = i_a_tmp - 1.7320508075688772 * l3 * theta1_tmp_tmp / 2.0;
    l_a_tmp = 2.0 * l1 * beta1 * j_a_tmp;
    m_a_tmp = 2.0 * l1 * theta1_tmp_tmp * k_a_tmp;
    c_a = l_a_tmp + m_a_tmp;
    d_a = beta2 - 0.8660254037844386 * l3 * beta1;
    e_a = i_a_tmp - 0.8660254037844386 * l3 * theta1_tmp_tmp;
    d_a = (((d_a * d_a + e_a * e_a) + b_a_tmp) - h_a_tmp) + a_tmp;
    beta1 = std::cos(beta3);
    theta1_tmp_tmp = std::sin(beta3);
    beta2 = (x + ra * beta1) - rb * beta1;
    i_a_tmp = (y + ra * theta1_tmp_tmp) - rb * theta1_tmp_tmp;
    n_a_tmp = beta2 - 1.7320508075688772 * l3 * beta1 / 2.0;
    o_a_tmp = i_a_tmp - 1.7320508075688772 * l3 * theta1_tmp_tmp / 2.0;
    p_a_tmp = 2.0 * l1 * beta1 * n_a_tmp;
    q_a_tmp = 2.0 * l1 * theta1_tmp_tmp * o_a_tmp;
    e_a = p_a_tmp + q_a_tmp;
    beta3 = beta2 - 0.8660254037844386 * l3 * beta1;
    beta1 = i_a_tmp - 0.8660254037844386 * l3 * theta1_tmp_tmp;
    beta3 = (((beta3 * beta3 + beta1 * beta1) + b_a_tmp) - h_a_tmp) + a_tmp;
    beta2 = 2.0 * l1 * g_a_tmp;
    beta1 = g_a_tmp * g_a_tmp;
    theta1_tmp_tmp = 4.0 * b_a_tmp * beta1;
    *th1 =
        -2.0 *
        std::atan(
            (beta2 + std::sqrt((a * a + theta1_tmp_tmp) - b_a * b_a)) /
            ((((((c_a_tmp * c_a_tmp + d_a_tmp * d_a_tmp) + b_a_tmp) - h_a_tmp) +
                beta1) +
                e_a_tmp) +
                f_a_tmp)) *
        180.0 / 3.1415926535897931;
    *th2 =
        -2.0 *
        std::atan(
            (beta2 + std::sqrt((c_a * c_a + theta1_tmp_tmp) - d_a * d_a)) /
            ((((((j_a_tmp * j_a_tmp + k_a_tmp * k_a_tmp) + b_a_tmp) - h_a_tmp) +
                beta1) +
                l_a_tmp) +
                m_a_tmp)) *
        180.0 / 3.1415926535897931;
    *th3 =
        -2.0 *
        std::atan(
            (beta2 + std::sqrt((e_a * e_a + theta1_tmp_tmp) - beta3 * beta3)) /
            ((((((n_a_tmp * n_a_tmp + o_a_tmp * o_a_tmp) + b_a_tmp) - h_a_tmp) +
                beta1) +
                p_a_tmp) +
                q_a_tmp)) *
        180.0 / 3.1415926535897931;


	*th1 = zero_angle - *th1;
	*th2 = zero_angle - *th2;
	*th3 = zero_angle - *th3;
}

void RobotDelta::Delta_Forward(double l1, double l3, double the3, double the2,
                               double rb, double l2, double ra, double BETA3, double BETA2,
                               double BETA1, double the1, double zero_angle, double* z, double* y, double* x)
{
    double a2h;
    double a_tmp;
    double a_tmp_tmp;
    double ab_a_tmp;
    double b_a_tmp;
    double b_a_tmp_tmp;
    double b_x1_tmp_tmp;
    double b_y1_tmp_tmp;
    double bb_a_tmp;
    double beta1;
    double beta2;
    double beta3;
    double c_a_tmp;
    double c_a_tmp_tmp;
    double cb_a_tmp;
    double d_a_tmp;
    double d_a_tmp_tmp;
    double db_a_tmp;
    double ddd11;
    double ddd21;
    double e_a_tmp;
    double eb_a_tmp;
    double f_a_tmp;
    double fb_a_tmp;
    double g_a_tmp;
    double h_a_tmp;
    double i_a_tmp;
    double j_a_tmp;
    double k_a_tmp;
    double l_a_tmp;
    double m_a_tmp;
    double n_a_tmp;
    double o_a_tmp;
    double p_a_tmp;
    double q_a_tmp;
    double r_a_tmp;
    double s_a_tmp;
    double t_a_tmp;
    double th1;
    double th2;
    double th3;
    double u_a_tmp;
    double v_a_tmp;
    double w_a_tmp;
    double x1_tmp_tmp;
    double x_a_tmp;
    double y1_tmp_tmp;
    double y_a_tmp;

	double theta1 = zero_angle - the1;
	double theta2 = zero_angle - the2;
	double theta3 = zero_angle - the3;


    beta1 = BETA1 * 3.1415926535897931 / 180.0;
    beta2 = BETA2 * 3.1415926535897931 / 180.0;
    beta3 = BETA3 * 3.1415926535897931 / 180.0;
    th1 = theta1 * 3.1415926535897931 / 180.0;
    th2 = theta2 * 3.1415926535897931 / 180.0;
    th3 = theta3 * 3.1415926535897931 / 180.0;
    x1_tmp_tmp = std::cos(beta1);
    b_x1_tmp_tmp = std::cos(beta2);
    ddd21 = std::cos(th1);
    a2h = std::cos(th2);
    y1_tmp_tmp = std::sin(beta1);
    b_y1_tmp_tmp = std::sin(beta2);
    a_tmp = l1 * std::sin(th1);
    b_a_tmp = l1 * std::sin(th2);
    c_a_tmp = a_tmp - b_a_tmp;
    d_a_tmp = ra * y1_tmp_tmp;
    e_a_tmp = rb * y1_tmp_tmp;
    a_tmp_tmp = 1.7320508075688772 * l3 * y1_tmp_tmp;
    f_a_tmp = a_tmp_tmp / 2.0;
    g_a_tmp = l1 * y1_tmp_tmp * ddd21;
    h_a_tmp = ra * b_y1_tmp_tmp;
    i_a_tmp = rb * b_y1_tmp_tmp;
    b_a_tmp_tmp = 1.7320508075688772 * l3 * b_y1_tmp_tmp;
    j_a_tmp = b_a_tmp_tmp / 2.0;
    k_a_tmp = l1 * b_y1_tmp_tmp * a2h;
    l_a_tmp =
        ((((((d_a_tmp - h_a_tmp) - e_a_tmp) + i_a_tmp) - f_a_tmp) + j_a_tmp) -
            g_a_tmp) +
        k_a_tmp;
    m_a_tmp = ra * x1_tmp_tmp;
    n_a_tmp = rb * x1_tmp_tmp;
    o_a_tmp = l1 * x1_tmp_tmp * ddd21;
    c_a_tmp_tmp = 1.7320508075688772 * l3 * x1_tmp_tmp;
    p_a_tmp = c_a_tmp_tmp / 2.0;
    q_a_tmp = ra * b_x1_tmp_tmp;
    r_a_tmp = rb * b_x1_tmp_tmp;
    s_a_tmp = l1 * b_x1_tmp_tmp * a2h;
    d_a_tmp_tmp = 1.7320508075688772 * l3 * b_x1_tmp_tmp;
    t_a_tmp = d_a_tmp_tmp / 2.0;
    u_a_tmp =
        ((((((m_a_tmp - q_a_tmp) - n_a_tmp) + r_a_tmp) - o_a_tmp) + s_a_tmp) -
            p_a_tmp) +
        t_a_tmp;
    v_a_tmp = l1 * std::sin(th3);
    ddd21 = a_tmp - v_a_tmp;
    w_a_tmp = std::sin(beta3);
    x_a_tmp = std::cos(th3);
    y_a_tmp = ra * w_a_tmp;
    a2h = rb * w_a_tmp;
    ddd11 = 1.7320508075688772 * l3 * w_a_tmp / 2.0;
    ab_a_tmp = l1 * w_a_tmp * x_a_tmp;
    f_a_tmp = ((((((d_a_tmp - y_a_tmp) - e_a_tmp) + a2h) - f_a_tmp) + ddd11) -
        g_a_tmp) +
        ab_a_tmp;
    bb_a_tmp = std::cos(beta3);
    cb_a_tmp = ra * bb_a_tmp;
    db_a_tmp = rb * bb_a_tmp;
    eb_a_tmp = l1 * bb_a_tmp * x_a_tmp;
    fb_a_tmp = 1.7320508075688772 * l3 * bb_a_tmp / 2.0;
    p_a_tmp =
        ((((((m_a_tmp - cb_a_tmp) - n_a_tmp) + db_a_tmp) - o_a_tmp) + eb_a_tmp) -
            p_a_tmp) +
        fb_a_tmp;
    v_a_tmp = b_a_tmp - v_a_tmp;
    j_a_tmp = ((((((h_a_tmp - y_a_tmp) - i_a_tmp) + a2h) - j_a_tmp) + ddd11) -
        k_a_tmp) +
        ab_a_tmp;
    t_a_tmp =
        ((((((q_a_tmp - cb_a_tmp) - r_a_tmp) + db_a_tmp) - s_a_tmp) + eb_a_tmp) -
            t_a_tmp) +
        fb_a_tmp;
    ab_a_tmp = std::sqrt((ddd21 * ddd21 + f_a_tmp * f_a_tmp) + p_a_tmp * p_a_tmp);
    cb_a_tmp =
        std::sqrt((c_a_tmp * c_a_tmp + l_a_tmp * l_a_tmp) + u_a_tmp * u_a_tmp);
    a2h = std::sqrt((v_a_tmp * v_a_tmp + j_a_tmp * j_a_tmp) + t_a_tmp * t_a_tmp);
    db_a_tmp = cb_a_tmp / 2.0;
    ddd21 = ab_a_tmp / 2.0;
    y_a_tmp = a2h / 2.0;
    ddd11 = db_a_tmp + ddd21;
    a2h = cb_a_tmp * ab_a_tmp * a2h /
        (4.0 * std::sqrt(((ddd21 - db_a_tmp) + y_a_tmp) *
            ((db_a_tmp - ddd21) + y_a_tmp) * (ddd11 - y_a_tmp) *
            (ddd11 + y_a_tmp)));
    f_a_tmp = v_a_tmp * u_a_tmp -
        c_a_tmp * (((((((ra * std::cos(beta2) - ra * std::cos(beta3)) -
            rb * std::cos(beta2)) +
            rb * std::cos(beta3)) -
            l1 * std::cos(beta2) * std::cos(th2)) +
            l1 * std::cos(beta3) * std::cos(th3)) -
            1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) +
            1.7320508075688772 * l3 * bb_a_tmp / 2.0);
    ddd21 =
        -(u_a_tmp * (((((((ra * std::sin(beta2) - ra * w_a_tmp) -
            rb * std::sin(beta2)) +
            rb * w_a_tmp) -
            1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) +
            1.7320508075688772 * l3 * w_a_tmp / 2.0) -
            l1 * std::sin(beta2) * std::cos(th2)) +
            l1 * w_a_tmp * x_a_tmp) -
            (((((((ra * std::cos(beta2) - ra * bb_a_tmp) - rb * std::cos(beta2)) +
                rb * bb_a_tmp) -
                l1 * std::cos(beta2) * std::cos(th2)) +
                l1 * bb_a_tmp * x_a_tmp) -
                1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) +
                1.7320508075688772 * l3 * bb_a_tmp / 2.0) *
            l_a_tmp) *
        l_a_tmp -
        f_a_tmp * c_a_tmp;
    ab_a_tmp = c_a_tmp * (((((((ra * std::sin(beta2) - ra * std::sin(beta3)) -
        rb * std::sin(beta2)) +
        rb * std::sin(beta3)) -
        1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) +
        1.7320508075688772 * l3 * w_a_tmp / 2.0) -
        l1 * std::sin(beta2) * std::cos(th2)) +
        l1 * std::sin(beta3) * std::cos(th3));
    y_a_tmp = v_a_tmp * l_a_tmp;
    j_a_tmp = y_a_tmp - ab_a_tmp;
    c_a_tmp = ((((((((ra * std::cos(beta1) - ra * std::cos(beta2)) -
        rb * std::cos(beta1)) +
        rb * std::cos(beta2)) -
        l1 * std::cos(beta1) * std::cos(th1)) +
        l1 * std::cos(beta2) * std::cos(th2)) -
        1.7320508075688772 * l3 * x1_tmp_tmp / 2.0) +
        1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) *
        (((((((ra * std::sin(beta2) - ra * std::sin(beta3)) -
            rb * std::sin(beta2)) +
            rb * std::sin(beta3)) -
            1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) +
            1.7320508075688772 * l3 * w_a_tmp / 2.0) -
            l1 * std::sin(beta2) * std::cos(th2)) +
            l1 * std::sin(beta3) * std::cos(th3)) -
        (((((((ra * std::cos(beta2) - ra * std::cos(beta3)) -
            rb * std::cos(beta2)) +
            rb * std::cos(beta3)) -
            l1 * std::cos(beta2) * std::cos(th2)) +
            l1 * std::cos(beta3) * std::cos(th3)) -
            1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) +
            1.7320508075688772 * l3 * bb_a_tmp / 2.0) *
        (((((((ra * std::sin(beta1) - ra * std::sin(beta2)) -
            rb * std::sin(beta1)) +
            rb * std::sin(beta2)) -
            1.7320508075688772 * l3 * y1_tmp_tmp / 2.0) +
            1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) -
            l1 * std::sin(beta1) * std::cos(th1)) +
            l1 * std::sin(beta2) * std::cos(th2))) *
        u_a_tmp -
        j_a_tmp * c_a_tmp;
    j_a_tmp = f_a_tmp * u_a_tmp + j_a_tmp * l_a_tmp;
    a2h *= a2h;
    ddd11 = std::sqrt(a2h - db_a_tmp * db_a_tmp) /
        std::sqrt((ddd21 * ddd21 + c_a_tmp * c_a_tmp) + j_a_tmp * j_a_tmp);
    l_a_tmp = ab_a_tmp - y_a_tmp;
    p_a_tmp = (((((((ra * std::cos(beta2) - ra * std::cos(beta3)) -
        rb * std::cos(beta2)) +
        rb * std::cos(beta3)) -
        l1 * std::cos(beta2) * std::cos(th2)) +
        l1 * std::cos(beta3) * std::cos(th3)) -
        1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) +
        1.7320508075688772 * l3 * bb_a_tmp / 2.0) *
        (((((((ra * std::sin(beta1) - ra * std::sin(beta2)) -
            rb * std::sin(beta1)) +
            rb * std::sin(beta2)) -
            1.7320508075688772 * l3 * y1_tmp_tmp / 2.0) +
            1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) -
            l1 * std::sin(beta1) * std::cos(th1)) +
            l1 * std::sin(beta2) * std::cos(th2)) -
        (((((((ra * std::cos(beta1) - ra * std::cos(beta2)) -
            rb * std::cos(beta1)) +
            rb * std::cos(beta2)) -
            l1 * std::cos(beta1) * std::cos(th1)) +
            l1 * std::cos(beta2) * std::cos(th2)) -
            1.7320508075688772 * l3 * x1_tmp_tmp / 2.0) +
            1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) *
        (((((((ra * std::sin(beta2) - ra * std::sin(beta3)) -
            rb * std::sin(beta2)) +
            rb * std::sin(beta3)) -
            1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) +
            1.7320508075688772 * l3 * w_a_tmp / 2.0) -
            l1 * std::sin(beta2) * std::cos(th2)) +
            l1 * std::sin(beta3) * std::cos(th3));
    ddd21 =
        std::sqrt(l2 * l2 - a2h) /
        std::sqrt((l_a_tmp * l_a_tmp + f_a_tmp * f_a_tmp) + p_a_tmp * p_a_tmp);
    *x =
        ((((((((n_a_tmp / 2.0 - q_a_tmp / 2.0) - m_a_tmp / 2.0) + r_a_tmp / 2.0) +
            o_a_tmp / 2.0) +
            s_a_tmp / 2.0) +
            c_a_tmp_tmp / 4.0) +
            d_a_tmp_tmp / 4.0) +
            (-((((((((ra * std::cos(beta1) - ra * std::cos(beta2)) -
                rb * std::cos(beta1)) +
                rb * std::cos(beta2)) -
                l1 * std::cos(beta1) * std::cos(th1)) +
                l1 * std::cos(beta2) * std::cos(th2)) -
                1.7320508075688772 * l3 * x1_tmp_tmp / 2.0) +
                1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) *
                (((((((ra * std::sin(beta2) - ra * std::sin(beta3)) -
                    rb * std::sin(beta2)) +
                    rb * std::sin(beta3)) -
                    1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) +
                    1.7320508075688772 * l3 * w_a_tmp / 2.0) -
                    l1 * std::sin(beta2) * std::cos(th2)) +
                    l1 * std::sin(beta3) * std::cos(th3)) -
                (((((((ra * std::cos(beta2) - ra * std::cos(beta3)) -
                    rb * std::cos(beta2)) +
                    rb * std::cos(beta3)) -
                    l1 * std::cos(beta2) * std::cos(th2)) +
                    l1 * std::cos(beta3) * std::cos(th3)) -
                    1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) +
                    1.7320508075688772 * l3 * bb_a_tmp / 2.0) *
                (((((((ra * std::sin(beta1) - ra * std::sin(beta2)) -
                    rb * std::sin(beta1)) +
                    rb * std::sin(beta2)) -
                    1.7320508075688772 * l3 * y1_tmp_tmp / 2.0) +
                    1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) -
                    l1 * std::sin(beta1) * std::cos(th1)) +
                    l1 * std::sin(beta2) * std::cos(th2))) *
                (((((((ra * std::sin(beta1) - ra * std::sin(beta2)) -
                    rb * std::sin(beta1)) +
                    rb * std::sin(beta2)) -
                    1.7320508075688772 * l3 * y1_tmp_tmp / 2.0) +
                    1.7320508075688772 * l3 * b_y1_tmp_tmp / 2.0) -
                    l1 * std::sin(beta1) * std::cos(th1)) +
                    l1 * std::sin(beta2) * std::cos(th2)) -
                ((l1 * std::sin(th2) - l1 * std::sin(th3)) *
                    (((((((ra * std::cos(beta1) - ra * std::cos(beta2)) -
                        rb * std::cos(beta1)) +
                        rb * std::cos(beta2)) -
                        l1 * std::cos(beta1) * std::cos(th1)) +
                        l1 * std::cos(beta2) * std::cos(th2)) -
                        1.7320508075688772 * l3 * x1_tmp_tmp / 2.0) +
                        1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) -
                    (l1 * std::sin(th1) - l1 * std::sin(th2)) *
                    (((((((ra * std::cos(beta2) - ra * std::cos(beta3)) -
                        rb * std::cos(beta2)) +
                        rb * std::cos(beta3)) -
                        l1 * std::cos(beta2) * std::cos(th2)) +
                        l1 * std::cos(beta3) * std::cos(th3)) -
                        1.7320508075688772 * l3 * b_x1_tmp_tmp / 2.0) +
                        1.7320508075688772 * l3 * bb_a_tmp / 2.0)) *
                (l1 * std::sin(th1) - l1 * std::sin(th2))) *
            ddd11) +
        l_a_tmp * ddd21;
    *y =
        ((((((((e_a_tmp / 2.0 - h_a_tmp / 2.0) - d_a_tmp / 2.0) + i_a_tmp / 2.0) +
            a_tmp_tmp / 4.0) +
            b_a_tmp_tmp / 4.0) +
            g_a_tmp / 2.0) +
            k_a_tmp / 2.0) +
            c_a_tmp * ddd11) +
        f_a_tmp * ddd21;
    *z = (((-l3 / 2.0 - a_tmp / 2.0) - b_a_tmp / 2.0) + j_a_tmp * ddd11) +
        p_a_tmp * ddd21;
}


